//
// Created by ze on 2023/11/2.
//

#ifndef ENCODER_UPRE_LOCATION_H
#define ENCODER_UPRE_LOCATION_H
/*********标定全场的步骤**************
*1.先将T1调成0度 ，DETA_T调成90度。
*2.把车拉出去推一圈，随便推。
*3.用上位机程序推出DETA_T的角度。
*4.再沿着某个轴推出一个直线，最后到的点在直线上就行。
*5.用上位机推出的角度算出T1
*6.全场标定完毕，在主机中进行平移中心即可。
**************************************/

/***************************************************************
*		    		需要标定参数，按照标定顺序排列
****************************************************************/
#include "UPRE_GLOBAL_DEFINE.h"

//X轴    脉冲值转换成毫米
#define  PULSE2DIS_ENCODE1 (0.07682846f*1.0075105f * 0.50010746f * 0.99726836f)/*(8495.f/110571.f)*(9122/9054)*(9162/18302)*/
//Y轴    脉冲值转换成毫米
#define  PULSE2DIS_ENCODE2 (0.07808927f * 0.9973800f*0.500601027f)/*8741/111936 * 9136/9160*/
////X轴    脉冲值转换成毫米
//#define  PULSE2DIS_ENCODE1 (1.f)
////Y轴    脉冲值转换成毫米
//#define  PULSE2DIS_ENCODE2 (1.f)

//X轴    等比例缩放
#define  ENCODER_DIS1       1.f
//Y轴    等比例缩放
#define  ENCODER_DIS2       1.f


//T2-T1  89.525
#define   DETA_T                    (85.698534959.f)
//码盘1与X轴夹角45.1
#define   Tt1					    (-42.439520387)
//码盘2与X轴夹角
#define   Tt2 						(43.259014572f)//z
#define sint1       sinf(location.theta1)
#define sint2       sinf(location.theta2)
#define cost1       cosf(location.theta1)
#define cost2       cosf(location.theta2)
//#define _L1          (location.R1 * location.ALPHA)
//#define _L2          (location.R2 * location.ALPHA)
#define  DIS1       (location.DIS_1 * PULSE2DIS_ENCODE1)
#define  DIS2       (location.DIS_2 * PULSE2DIS_ENCODE2)
typedef struct _Location
{
    float DIS_1, DIS_2;//DIS_1:旋转前的x坐标  DIS_2:旋转前的Y坐标
    float ALPHA, gyro_radian, alphaed, Center_X, Center_Y ,Whole_X, Whole_Y, Sin_12, Sin_21;
    float R1, R2, theta1, theta2;
    float fail1,fail2;
    float buf_x,buf_y,theta;
    float Last_whole_X,Last_whole_Y;//全场坐标的微分
    float m_spi_Posx,m_spi_Posy;//标定旋转前的全场坐标的微分
    float spi_Posx,spi_Posy;    //标定旋转前的全场坐标的积分
    float RECTANG_X,RECTANG_Y;
    float X_coder,Y_coder;
    float k1,k2,k3,k4;
    float m1,m2;                        //微分
    float dised_1,dised_2;
} Location;
extern Pose Robot_Pose;
void Encoder_On();
void Location_Init(void);
void cal_location(void);
extern Location location;

#endif //ENCODER_UPRE_LOCATION_H
